/**
  ******************************************************************************
  * @file
  * @brief 比例阀控制
  * @author luffy
  * @version V1.0.0
  * @date 2022/12/14
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "func_pid.h"
#include "switch.h"
#include "FreeRTOS.h"
#include "valve_ctl_task.h"
#include "pressuresensor.h"
#include "ParmManage.h"
#include "aclass_protocol.h"
#include "lwip_app.h"
#include "task.h"
#include "SYS.h"
/* Private defines -----------------------------------------------------------*/
#define	PRES_MAX_VAL	1000						//压力报警值
/* Private Struct  -----------------------------------------------------------*/

/* Private enum    -----------------------------------------------------------*/

/* Private Variable  ---------------------------------------------------------*/
static int   s_overDelayCnt  = 0;             //压力超时判断时间计数
VALVECTRL    BODYPRESCTRL;                    //基体腔压力控制参数
uint8_t      g_presState = 0;
static bool  s_first_ctr = true;
//Pointer

//Array

//Const

/*-----------------------------------------------------------------------------
                            内部函数声明
-----------------------------------------------------------------------------*/
static void  IsPresSteady(void);
static void  PresCtrl(void);
static void  IsPresOver(void);
static void  PresAlarm(void);
/*---------------------------------------------------------------------------------------
 函数原型: void PCON_Init(void)
 功    能: 启动压力控制线程
 输入参数: NA
 返 回 值: NA
 注意事项: 	
---------------------------------------------------------------------------------------*/
void PCON_Init(void)
{
    //初始化压力控制数据
//	PPItemRead(PP_VALVE_PID_P,         (uint8_t *)&(BODYPRESCTRL.pid.Kp),       4);
//	PPItemRead(PP_VALVE_PID_I,         (uint8_t *)&(BODYPRESCTRL.pid.Ki),       4);
//	PPItemRead(PP_VALVE_PID_D,         (uint8_t *)&(BODYPRESCTRL.pid.Kd),       4);
//	PPItemRead(PP_CELOM_PRE_THRESHOLD, (uint8_t *)&(BODYPRESCTRL.presAccuracy), 4); //压力控制精度
//	PPItemRead(PP_VALVE_PID_PERIOD,    (uint8_t *)&(BODYPRESCTRL.sampleCycle),  2);
//	PPItemRead(PP_CELOM_PRE,           (uint8_t *)&(BODYPRESCTRL.pid.SetVal),   4);
	
	BODYPRESCTRL.bleedPid.Kp = 0.00001;   //大流量抽气比例阀P:0.00001  小流量比例阀P:0.00008
	BODYPRESCTRL.bleedPid.Ki = 0.000002;  //大流量抽气比例阀I:0.000002
	
	BODYPRESCTRL.blowPid.Kp = 0.000009;   //小流量充气比例阀P:0.000002(初始0.5)   0.000005(速率650-1000 初始0.5) 0.000009(速率3000 初始0.5 20000步长) 后面PP0大充气量，不考虑速率时可以加大P或目标速率超调
	BODYPRESCTRL.blowPid.Ki = 0.000001;   //小流量充气比例阀I:0.000002
	BODYPRESCTRL.blowPid.Kd = 0.000000;
	BODYPRESCTRL.presAccuracy = 200.0;
	BODYPRESCTRL.sampleCycle = 400;
	
	BODYPRESCTRL.bleedPid.SetVal = 670;   //
	BODYPRESCTRL.blowPid.SetVal = 670;   //
	
	BODYPRESCTRL.bleedPid.outval_lim 	 = 1.0F;			
    BODYPRESCTRL.bleedPid.integral_lim  = 1000.0F;
    BODYPRESCTRL.blowPid.outval_lim 	 = 1.0F;			
    BODYPRESCTRL.blowPid.integral_lim  = 1000.0F;	
	
    BODYPRESCTRL.waitingTime       = 100;       //10S 周期：100ms
    BODYPRESCTRL.presFilterSave    = 0.0F;
    BODYPRESCTRL.presFilterK 	     = 1.0;     //系统采样时间为0.5S，则时间常数为1S
    BODYPRESCTRL.presSave		     = 0.0F;
    BODYPRESCTRL.valveVoltage 	 = 0.5F;      //小流量充气控制初始电压
    BODYPRESCTRL.pressure  	     = 0.0F;
	BODYPRESCTRL.speed           = 0.0;
    BODYPRESCTRL.readytimer        = 0;
	BODYPRESCTRL.overTime          = 0;
    BODYPRESCTRL.presStable        = false;  
	BODYPRESCTRL.presCtrlEn        = 0;
	
    BODYPRESCTRL.pressure 	  = pBody.P_sensor1;
    BODYPRESCTRL.presFilterSave = BODYPRESCTRL.pressure;
    BODYPRESCTRL.presSave 	  = BODYPRESCTRL.pressure;
	
	Func_Pid_Init(&(BODYPRESCTRL.bleedPid));
	Func_Pid_Init(&(BODYPRESCTRL.blowPid));
	
	//初始化驱动
	Dev_dac8830_Init(&dac8830);
}

/*---------------------------------------------------------------------------------------
 函数原型: void PCON_Thread(void *pvParameters)
 功    能: 压力控制线程执行函数
 输入参数: NA
 返 回 值: NA
 注意事项: 	
---------------------------------------------------------------------------------------*/
void PCON_Thread(void *pvParameters)
{
    while(1)
    {
		PPItemRead(PP_VALVE_PID_PERIOD, (uint8_t *)&(BODYPRESCTRL.sampleCycle), 2);
		if(BODYPRESCTRL.sampleCycle == 0)
		{
			BODYPRESCTRL.sampleCycle = 100;
		}
        vTaskDelay(pdMS_TO_TICKS(BODYPRESCTRL.sampleCycle));
        
		//更新压力
        BODYPRESCTRL.pressure 	  = pBody.P_sensor1;
		BODYPRESCTRL.speed = fabs(BODYPRESCTRL.pressure - BODYPRESCTRL.presSave) / (BODYPRESCTRL.sampleCycle/1000.0);
		BODYPRESCTRL.presFilterSave =  (1 - BODYPRESCTRL.presFilterK) * BODYPRESCTRL.presFilterSave + BODYPRESCTRL.presFilterK * BODYPRESCTRL.presSave;
		BODYPRESCTRL.presSave 	  = BODYPRESCTRL.pressure;
		
		//tcp_senddata((uint8_t *)&BODYPRESCTRL.pressure,4);
		//Aclass_Protocol_Send("pres", &BODYPRESCTRL.pressure, 4);
		
        //判断温控是否使能
        if(BODYPRESCTRL.presCtrlEn == 0)
        {
			Func_Pid_Init(&(BODYPRESCTRL.bleedPid));
			Func_Pid_Init(&(BODYPRESCTRL.blowPid));
			
			dac8830.Write(&dac8830,0.0, 0);
             
			s_first_ctr = true;
            continue;
        }
		else if(BODYPRESCTRL.presCtrlEn == 2)  //暂停PID
		{
			continue;
		}
		else if(s_first_ctr)
		{
			s_first_ctr = false;
			
			BODYPRESCTRL.valveVoltage 	 = 0.5F;      //小流量充气控制初始电压0.5（不同压差的初始电压也不同，后面精调需调试压差和电压的关系，同样电压，压差越小速率越慢）
		}
    
        //更新PID参数(callback函数)、目标值程序中计算 
		
        //判断压力是否稳定
        IsPresSteady();
        
        //判断是否压力控制超时
        IsPresOver();
        
        //比例阀电压控制
        PresCtrl();
        
        //压力阈值报警处理
        PresAlarm();
    }  
}

/*---------------------------------------------------------------------------------------
 函数原型:  static void IsPresSteady(void)
 功    能:  判断压力是否稳定
 输入参数:	NA
 输出参数:	NA
 返 回 值:	
 注意事项：	本函数每500mS调用一次,函数内部已经对温度稳定与否进行了判断，变量设置，所以无返回值
----------------------------------------------------------------------------------------*/
static void IsPresSteady(void)
{
    if((BODYPRESCTRL.pressure > (BODYPRESCTRL.bleedPid.SetVal - BODYPRESCTRL.presAccuracy)) && (BODYPRESCTRL.pressure < (BODYPRESCTRL.bleedPid.SetVal + BODYPRESCTRL.presAccuracy)))
    {
        if(BODYPRESCTRL.presStable == false)
        {
            if(BODYPRESCTRL.readytimer < BODYPRESCTRL.waitingTime) //waitingTime单位为2.5S，本函数每500mS调用一次
            {
                BODYPRESCTRL.readytimer++;
            }
            else
            {
                BODYPRESCTRL.readytimer = 0;
                BODYPRESCTRL.presStable = true;
				g_presState = 1;
            }
        }
        else
        {
            BODYPRESCTRL.readytimer = 0;  
        }   
    }
    else
    {
        if(BODYPRESCTRL.presStable == true)
        {
            //所设置时间内都不在精度范围内（突然温度不稳定了，也持续稳定时间后才认为不稳定）
            if(BODYPRESCTRL.readytimer < BODYPRESCTRL.waitingTime)
            {
                BODYPRESCTRL.readytimer++;
            }
            else
            {
                BODYPRESCTRL.readytimer = 0;
                BODYPRESCTRL.presStable = false;
				g_presState = 3;
            }
        }
        else//当在温度范围内计数时，突然发现温度不再范围内，则需要从0开始重新计数
        {
            BODYPRESCTRL.readytimer = 0;
        }  
    }

}

/*---------------------------------------------------------------------------------------
 函数原型:  static void PresCtrl(void)
 功    能:  压力控制
 输入参数:	NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
----------------------------------------------------------------------------------------*/
static void PresCtrl(void)
{
    float outval;
    uint8_t presMax;
	
//	PPItemRead(PP_VALVE_CUR_MAX, (uint8_t *)&presMax, 1);
//	
//	//目标温度合理性检查,确保目标温度在设置的范围之内
//	if(BODYPRESCTRL.pid.SetVal > presMax)
//	{
//		BODYPRESCTRL.pid.SetVal = presMax;
//	}	
//	else if(BODYPRESCTRL.pid.SetVal < 0)
//	{
//		BODYPRESCTRL.pid.SetVal = 0;
//	}

	switch(BODYPRESCTRL.pidSwitch)
	{
		case BLEED_PID:
			outval = BODYPRESCTRL.bleedPid.Math(&(BODYPRESCTRL.bleedPid),BODYPRESCTRL.speed);
			break;
		case BLOW_PID:
			outval = BODYPRESCTRL.blowPid.Math(&(BODYPRESCTRL.blowPid),BODYPRESCTRL.speed);
			break;
	}
	
	//位置式
//	outval += 1;
//	outval = outval / 2.0;
//	BODYPRESCTRL.valveVoltage = outval * 2.5f;             //转换为电压(比例阀电路I = VDAC * 249/(249+3300) V实际 = I * 150Ω)
	
//	if(outval <= -0.2)
//	{
//		outval = -0.2;
//	}
//	if(outval >= 0.2)
//	{
//		outval = 0.2;
//	}
	
	//增量式
	BODYPRESCTRL.valveVoltage += outval;
	
	if(BODYPRESCTRL.valveVoltage <= 0)
	{
		BODYPRESCTRL.valveVoltage = 0.0;
	}
	if(BODYPRESCTRL.valveVoltage >= 2.5)
	{
		BODYPRESCTRL.valveVoltage = 2.5;
	}
	
	dac8830.Write(&dac8830,BODYPRESCTRL.valveVoltage, 0);
	//dac8830.Write(&dac8830,0.5);  //1.2全开
}

/*---------------------------------------------------------------------------------------
 函数原型:  static void PresAlarm(void)
 功    能:  压力报警
 输入参数:	NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
----------------------------------------------------------------------------------------*/
static void PresAlarm(void)
{  
    if(BODYPRESCTRL.pressure >= PRES_MAX_VAL)
    {
        g_presState = 3;
    }
    else
    {
//        g_presState = 1;
    }
}

/*---------------------------------------------------------------------------------------
 函数原型:  static void IsPresOver(void)
 功    能:  压力控制超时处理
 输入参数:	NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
----------------------------------------------------------------------------------------*/
static void IsPresOver(void)
{ 
    if(BODYPRESCTRL.presStable == false)
    {
        s_overDelayCnt++;
        
        if(s_overDelayCnt >= BODYPRESCTRL.overTime)  
        {
            s_overDelayCnt = 0;
            
            //超时处理
            
            return;
        }
    }
	else
    {
		s_overDelayCnt = 0;
		//未超时处理
	}
	
    return;
    
}

/***********************************************************************************
 * @brief 压力控制参数设置
 * ex:
 * @par
 * None
 * @retval
 **********************************************************************************/
uint8_t PresTaskSet(uint8_t cmd, void *par)
{
	uint16_t i = 0;
	uint8_t *pr = par;
	
    switch (cmd) {
    case pres_task_set_alarm:
//		memcpy(&(BODYPRESCTRL.pid.SetVal), pr, sizeof(float));
//        PPItemIWrite(PP_CELOM_PRE, par, 4);
        break;

    case pres_task_set_target:
        memcpy(&(BODYPRESCTRL.bleedPid.SetVal), pr, sizeof(float));
        PPItemIWrite(PP_CELOM_PRE, par, 4);
        break;

    case pres_task_set_pid:
		memcpy(&(BODYPRESCTRL.bleedPid.Kp), &pr[0], sizeof(float));
		memcpy(&(BODYPRESCTRL.bleedPid.Ki), &pr[4], sizeof(float));
		memcpy(&(BODYPRESCTRL.bleedPid.Kd), &pr[8], sizeof(float));
	
        PPItemIWrite(PP_VALVE_PID_P, &pr[0], 4);
		PPItemIWrite(PP_VALVE_PID_P, &pr[4], 4);
		PPItemIWrite(PP_VALVE_PID_P, &pr[8], 4);
        break;

    default:
        break;
    }

    return 0;
}

/***********************************************************************************
 * @brief 压力控制参数读取
 * ex:
 * @par
 * None
 * @retval
 **********************************************************************************/
uint8_t PresTaskRead(uint8_t cmd, void *par)
{
	uint8_t *pr = par;
	
	switch (cmd)
	{
	case pres_task_read_pres:
		memcpy(pr, &BODYPRESCTRL.pressure, sizeof(float));
		break;

	case pres_task_read_alarm:
//		memcpy(pr, &BODYPRESCTRL.pressure, sizeof(float));
		break;

	case pres_task_read_target:
		memcpy(pr, &BODYPRESCTRL.bleedPid.SetVal, sizeof(float));
		break;

	case pres_task_read_pid:
		memcpy(&pr[0], &(BODYPRESCTRL.bleedPid.Kp), sizeof(float));
		memcpy(&pr[4], &BODYPRESCTRL.bleedPid.Ki, sizeof(float));
		memcpy(&pr[8], &BODYPRESCTRL.bleedPid.Kd, sizeof(float));
		break;

	default:
		break;
	}

	return 0;
}

/******************* (C) COPYRIGHT 2022 CIQTEK luffy *****END OF FILE****/
